/*
 * Queue.c: Implementation of a wait free queue
 */
#include "WFQueue.h"
#include "CAS.h"
#include <stdlib.h>
#include <stdio.h>

/**
 * Get the state from the states array
 * (this is used in order to avoid false-sharing)
 */
#define QUEUE_STATES(i) queue->states[i*NUM_BYTES_IN_CACHE_LINE]

/**
 * OpDesc (=state of a thread) has 4 fields:
 * 		phase     (long): saved in the field phase
 * 		pending   (bool): saved in the one before last bit of the field data
 * 		enqueueOp (bool): saved in the last bit of the field data
 * 		node     (Node*): saved in the field data, but 2 last bits are
 * 						  are used for pending and for enqueueOp
 * 	Data look like this:
 * 		bit 0: enqueueOp
 * 		bit 1: pending
 * 		bits 2-63: node
 */
#define getPending(data)   ((bool) ((((unsigned long long)data) & 0x2)>>1))
#define getEnqueueOp(data) ((bool)  (((unsigned long long)data) & 0x1))
#define getNode(data) 	   ((Node*)((((unsigned long long)data) | 3)-3))
#define setData(pendingB, enqueueOp, nodeP) (void*) \
		(((unsigned long long)nodeP)+ pendingB*2 + enqueueOp)

/**
 * Global Declarations
 */
inline void help_func		(WFQueue* queue, long long myTid, long phase);
inline void help_enq		(WFQueue* queue, long long myTid, long long tid,
		long phase);
inline void help_deq		(WFQueue* queue, long long myTid, long long tid,
		long phase);
inline void help_finish_enq	(WFQueue* queue, long long myTid);
inline void help_finish_deq	(WFQueue* queue, long long myTid);
inline bool isStillPending 	(WFQueue* queue, long long tid, long ph);
inline long maxPhase		(WFQueue* queue);

inline WFQueue* getQueue(unsigned int numOfThreads){
	WFQueue* queue = NULL;
	if (numOfThreads > 0){
		queue = (WFQueue*)malloc(sizeof(WFQueue));
		Node* dummyNode = (Node*)malloc(sizeof(Node));
		queue->states = (OpDesc*)malloc(sizeof(OpDesc)* numOfThreads *
				NUM_BYTES_IN_CACHE_LINE);
		queue->numOfThreads = numOfThreads;
		dummyNode->value= dummyNode->deqTid = dummyNode->enqTid = -1;
		dummyNode->next = NULL;
		queue->head = dummyNode;
		queue->tail = dummyNode;
#ifdef USE_HP
		queue->memoryManager = getMemoryManager(numOfThreads);
#endif

		for (int i = 0; i < numOfThreads; i++) {
			QUEUE_STATES(i).phase = -1;
			QUEUE_STATES(i).data = setData(false, true, NULL);
		}
	}
	return queue;
}

/**
 * delete the queue.
 * free all memory.
 */
inline void deleteQueue(WFQueue* queue){
	if (NULL == queue) return;
#ifdef USE_HP
	deleteMemoryManager(queue->memoryManager);
#endif
	Node* currNode;
	while(NULL != queue->head){
		currNode = queue->head;
		queue->head = queue->head->next;
		free(currNode);
	}
	free(queue->states);
	free(queue);
}

/**
 * Enqueue a new value to the queue
 */
inline void enq(WFQueue* queue, long long myTid, int value) {
	if (NULL == queue) return;
	long phase = maxPhase(queue) + 1;
	//if myTid is valid
	if (queue->numOfThreads > myTid){
		//Allocate new node
		Node* newNode = (Node*)malloc(sizeof(Node));
		newNode->value = value;
		newNode->enqTid = myTid;
		newNode->deqTid = -1;
		newNode->next = NULL;

		//update opDesc
		QUEUE_STATES(myTid).phase = phase;
		QUEUE_STATES(myTid).data = setData(true, true, newNode);

		//help
		help_func(queue, myTid, phase);
		help_finish_enq(queue, myTid);
	}
#ifdef USE_HP
	MemoryManager* mm = queue->memoryManager;
	cleanHP(GET_THREAD_DATA(myTid));
#endif
}

/**
 * Dequeue a node from the queue
 */
inline int deq(WFQueue* queue, long long myTid){
	int res = -1;
	if (NULL == queue) return res;
	long phase = maxPhase(queue) + 1;

	//if myTid is valid
	if (queue->numOfThreads > myTid){
		//update opDesc
		QUEUE_STATES(myTid).phase = phase;
		QUEUE_STATES(myTid).data = setData(true, false, NULL);

		//help
		help_func(queue, myTid, phase);
		help_finish_deq(queue, myTid);

		//get result
		Node* node = getNode(QUEUE_STATES(myTid).data);
		if (node != NULL)
		{
			res = node->next->value;

#ifdef USE_HP
			//mark node for deletion
			retireNode(queue->memoryManager, node, myTid);
#endif
		}
	}
#ifdef USE_HP
	//remove HP
	MemoryManager* mm = queue->memoryManager;
	cleanHP(GET_THREAD_DATA(myTid));
#endif
	return res;
}

inline void help_func(WFQueue* queue, long long myTid, long phase) {
	for (int i = 0; i < queue->numOfThreads; i++) {
		void* data =  QUEUE_STATES(i).data;
		bool pending = getPending(data);
		bool enqueueOp = getEnqueueOp(data);
		long curPhase = QUEUE_STATES(i).phase;
		//if thread needs help
		if (pending && curPhase <= phase) {
			if (enqueueOp) {
				help_enq(queue, myTid, i, phase);
			} else {
				help_deq(queue, myTid, i, phase);
			}
		}
	}
}


inline void help_enq(WFQueue* queue, long long myTid, long long tid,
		long phase) {
	//save HP
	while (isStillPending(queue, tid, phase)) {
		Node* last = queue->tail;
#ifdef USE_HP
		MemoryManager* mm = queue->memoryManager;
		GET_THREAD_DATA(myTid).hp0 = last;
		if (last !=queue->tail)
			continue;
#endif
		Node* next = last->next;
		//if queue tail wasn't changed
		if (last == queue->tail) {
			if (next == NULL) {
				Node* node = getNode(QUEUE_STATES(tid).data);
				if (isStillPending(queue, tid, phase)) {
					COUNT_CAS;
					if(CAS(&(last->next),next,node) )
					{
						help_finish_enq(queue, myTid);
						return;
					}
				}
			} else {//node was already inserted to queue
				help_finish_enq(queue, myTid);
			}
		}
	}
}

inline void help_finish_enq(WFQueue* queue, long long myTid) {
	//save HP
	Node* last = queue->tail;
#ifdef USE_HP
	MemoryManager* mm = queue->memoryManager;
	GET_THREAD_DATA(myTid).hp1 = last;
	if (last !=queue->tail)
		return;
#endif
	Node* next = last->next;
#ifdef USE_HP
	GET_THREAD_DATA(myTid).hp2 = next;
	if (next !=queue->tail->next)
		return;
#endif
	if (next != NULL) {//if finish wasn't performed yet
		long tid = next->enqTid;
		Node* node = getNode(QUEUE_STATES(tid).data);
		if (last == queue->tail && node == next) {
			COUNT_CAS;
			CAS(&QUEUE_STATES(tid).data,
					setData(true, true, node),
					setData(false, true, node) );
			COUNT_CAS;
			CAS(&queue->tail, last, next);
		}
	}
}

inline void help_deq(WFQueue* queue, long long myTid, long long tid,
		long phase) {
	while (isStillPending(queue, tid, phase)) {
		//save HP
		Node* first = queue->head;
#ifdef USE_HP
		MemoryManager* mm = queue->memoryManager;
		GET_THREAD_DATA(myTid).hp0 = first;
		if (first !=queue->head)
			continue;
#endif
		Node* last = queue->tail;
		Node* next = first->next;
#ifdef USE_HP
		GET_THREAD_DATA(myTid).hp1 = next;
		if (next != queue->head->next)
			continue;
#endif
		if (first == queue->head && next == first->next) {
			if (first == last) {//queue empty
				if(next == NULL){//help_deq for empty queue wasn't performed yet
					void* data = QUEUE_STATES(tid).data;
					if (last == queue->tail && isStillPending(queue,tid, phase)) {
						COUNT_CAS;
						CAS(&QUEUE_STATES(tid).data,
								setData(true, false, getNode(data)),
								setData(false, false, NULL) );
					}
				} else {//help_deq for empty queue was performed already
					help_finish_enq(queue, myTid);
				}
			} else {//queue isn't empty
				Node* node = getNode(QUEUE_STATES(tid).data);
				if (!isStillPending(queue, tid, phase)) break;
				if (first == queue->head && node != first) {
					COUNT_CAS;
					if (! CAS(&QUEUE_STATES(tid).data,
							setData(true, false, node),
							setData(true, false, first) )){
						continue;
					}
				}
				COUNT_CAS;
				CAS(&(first->deqTid), -1, tid);
				help_finish_deq(queue, myTid);
			}
		}
	}
}


inline void help_finish_deq(WFQueue* queue, long long myTid){
	//save HP
	Node* first = queue->head;
#ifdef USE_HP
	MemoryManager* mm = queue->memoryManager;
	GET_THREAD_DATA(myTid).hp2 = first;
	if (first != queue->head)
		return;
#endif
	Node* next = first->next;
	long tid = first->deqTid;

	if (tid != -1) {//if help_deq was performed
		if (first == queue->head && next != NULL) {
#ifdef USE_HP
			COUNT_CAS;
			if (!CAS(&GET_THREAD_DATA(tid).hp3,
					NULL, next)){
				Node* hp = GET_THREAD_DATA(tid).hp3;
				if (hp != next && QUEUE_STATES(tid).data == setData(true, false, first)){
					COUNT_CAS;
					CAS(&GET_THREAD_DATA(tid).hp3,
										hp, next);
				}
			}
#endif
			COUNT_CAS;
			CAS(&QUEUE_STATES(tid).data,
					setData(true, false, first),
					setData(false, false, first) );
			COUNT_CAS;
			CAS(&queue->head, first,next);
		}
	}
}


inline bool isStillPending(WFQueue* queue, long long tid, long ph) {
	void* data = QUEUE_STATES(tid).data;
	long phase = QUEUE_STATES(tid).phase;
	return getPending(data) && phase == ph;
}

/**
 * Find the maximal phase that is used in the queue
 */
inline long maxPhase(WFQueue* queue) {
	long maxPhase = -1;
	int i;
	for (i = 0; i < queue->numOfThreads; i++) {
		long phase = QUEUE_STATES(i).phase;
		if (phase > maxPhase) {
			maxPhase = phase;
		}
	}
	return maxPhase;
}

inline void printQueue(WFQueue* queue){
	Node* node = queue->head->next;
	printf("Queue = ");
	while(node){
		printf("%d, ",node->value);
		node = node->next;
	}
	printf("\n");
}
inline void printQueueSize(WFQueue* queue){
	Node* node = queue->head->next;
	int size = 0;
	while(node){
		size++;
		node = node->next;
	}
	printf("queue size=%d\n",size);
}
